/*******************************************************************************
*
*   Copyright ©2009 Alexandru Marin
*
*   This file is part of nwave/pMRI C library.
*
*   This project started as an MSc degree project during a
*   stage at the Paris-Est university, under joint supervision of prof.
*   Bogdan Ionescu (Image Analysis and Processing Laboratory, "Politehica"
*   University of Bucharest), Caroline Chaux and Jean-Christophe Pesquet
*   (Laboratoire d'Informatique, Université Paris-Est, Institut Gaspard Monge).
*
*   nwave/pMRI C libray is free software: you can redistribute it and/or modify
*   it under the terms of the GNU General Public License as published by
*   the Free Software Foundation, either version 2 of the License, or
*   (at your option) any later version.
*
*   nwave/pMRI C libray is distributed in the hope that it will be useful,
*   but WITHOUT ANY WARRANTY; without even the implied warranty of
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*   GNU General Public License for more details.
*
*   You should have received a copy of the GNU General Public License
*   along with nwave/pMRI C library.  If not, see <http://www.gnu.org/licenses/>.
*
******************************************************************************/

#include <stdio.h>
#include <complex.h>

#include "dwt_core.h"
#include "dwt_utils.h"
#include "mri_types.h"
#include <stdlib.h>
#include <assert.h>
#include <math.h>

void dwt_real_dec_along_dim(float* data, mri_matrix_descriptor_t* desc, int dim, dwt_filter_t* filter, dwt_border_t border_strategy, int* curr_signal_lens);
inline void dwt_real_dec_1d(float* data, int signallen, int gap, float** filters, int nbands, int filterlen, dwt_border_t border_strategy, float* wsvector);
inline void dwt_real_dec_1d_cdjv(float* data, int signallen, int gap, float** filters, dwt_cdjv_t* cdjv, int filterlen, float* wsvector);

void dwt_real_rec_along_dim(float* data, mri_matrix_descriptor_t* desc, int dim, dwt_filter_t* filter, dwt_border_t border_strategy, int* curr_signal_lens);
inline void dwt_real_rec_1d(float* data, int signallen, int gap, float** filters, int nbands, int filterlen, dwt_border_t border_strategy, float** wsvectors);
inline void dwt_real_rec_1d_cdjv(float* data, int signallen, int gap, float** filters, dwt_cdjv_t* cdjv, int filterlen, int padlen, float** wsvectors, float** edges);

void dwt_real_cond_cdjv_along_dim(float* data, int dim, mri_matrix_descriptor_t* desc, int degree, float** leftmat, float** rightmat);
void dwt_real_cond_cdjv_1d(float* data, int gap, int len, int degree, float** leftmat, float** rightmat, float* wsvector);
inline float dotprod_real(int len, float* a, float* b, int n);


void dwt_real_decomposition(float* data, mri_matrix_descriptor_t* desc, dwt_params_t* params) {

	int i;

	/**
	 * Checking assumptions validity
	 */

	// The number of dimensions (DWT and input data) must agree
	assert(desc->ndims==params->ndims);

	// Checks per dimension
	for (i = 0; i < desc->ndims; i++) {

		if(params->res_levels[i] == 0)
			continue;

		// The data length must be a multiple of nbands^resolution
		assert(desc->dims[i]%(int)pow(params->filters[i]->nbands,params->res_levels[i]) == 0);

		// At the highest resolution level we need at least filterlen elements left
		assert(desc->dims[i]/(int)pow(params->filters[i]->nbands,params->res_levels[i]) >= params->filters[i]->length);

		// CDJV check - only 2band transform
		if(params->border_strategies[i] == BORDER_CDJV || params->border_strategies[i] == BORDER_CDJV_COND) {
			assert(params->filters[i]->nbands == 2);
			assert(params->filters[i]->cdjv != NULL);
		}
	}


	// In case there is any CDJV wavelet for the intervals
	// do the preconditioning (if the flag is set)
//	printf("Preconditioning...\n");fflush(stdout);
	for(i=0;i<desc->ndims;i++)
		if(params->border_strategies[i] == BORDER_CDJV_COND)
			dwt_real_cond_cdjv_along_dim(data, i, desc,
					params->filters[i]->cdjv->degree,
					params->filters[i]->cdjv->leftprecond,
					params->filters[i]->cdjv->rightprecond);


//	printf("Initializing resolution level vector...\n");fflush(stdout);

	// This vector holds the current level of resolution for each dimension
	int* curr_res_levels = (int*) malloc(params->ndims * sizeof(int));
	assert(curr_res_levels!=NULL);

	// This is the needed number of resolution iterations - corresponding to
	// the dimension with the maximum number of res_levels
	int max_res_level = 0;

	// This vector holds the current signal length for each dimension; as the
	// resolution level increases, it will hold the length of "approximations"
	int* curr_signal_lens = (int*) malloc(params->ndims * sizeof(int));

	// Initialize resolution levels, signal lengths and compute max.
	for (i = 0; i < desc->ndims; i++) {

		curr_res_levels[i] = 0;

		curr_signal_lens[i] = desc->dims[i];

		if (params->res_levels[i] > max_res_level)
			max_res_level = params->res_levels[i];

	}

//	printf("max_res_level=%d, Start DWT...\n", max_res_level);fflush(stdout);

	// Loop counters
	int rl, dim;

	// Iterate through all dimensions until the desired resolution level is achieved
	for (rl = 0; rl < max_res_level; rl++) {

		for (dim = 0; dim < desc->ndims; dim++)
			curr_signal_lens[dim] = desc->dims[dim] / pow(params->filters[dim]->nbands, curr_res_levels[dim]);

		for (dim = 0; dim < desc->ndims; dim++) {
			if (curr_res_levels[dim] < params->res_levels[dim]) {
				// On this dimension we still need to apply the transform
				dwt_real_dec_along_dim(data, desc, dim, params->filters[dim], params->border_strategies[dim], curr_signal_lens);
				curr_res_levels[dim]++;
			}
		}
	}

	free(curr_res_levels);
	free(curr_signal_lens);

}

/**
 * Performs DWT along dimension dim of the multidimensional matrix data
 */
void dwt_real_dec_along_dim(float* data, mri_matrix_descriptor_t* desc, int dim, dwt_filter_t* filter, dwt_border_t border_strategy, int* curr_signal_lens) {

//	printf("Applying DWT along dimension #%d of the input data using filter %s"
//			" and border strategy %s\n",
//			dim+1, filter->name, DWT_BORDER_TO_STR(border_strategy));
//	printf("Current signal lenghts on each dimension:\n");
//	int cntr;
//	for(cntr=0;cntr<desc->ndims;cntr++)
//		printf(" %d ", curr_signal_lens[cntr]);
//	printf("\nPrecalculating variables...\n");fflush(stdout);

	/**
	 * Some useful stuff precalculated in local variables, for speed and simplicity
	 */
	int signallen = curr_signal_lens[dim];
	int gap = desc->gaps[dim];
	float** filters = filter->d_filters;
	dwt_cdjv_t* cdjv = filter->cdjv;
	int nbands = filter->nbands;
	int filterlen = filter->length;

//	printf("=> signallen=%d, gap=%d\n",signallen, gap);

	/**
	 * We now need to apply the DWT on each 1D vector of this dimension, which
	 * implies iterating through the space of all the other dimensions. This
	 * could be implemented for the n-dimensional case, but would waste a lot
	 * of time with iterating an n-1 dimensional space. Therefore, each case
	 * is implemented separately
	 */

	int i, j, k;

	if (border_strategy == BORDER_CDJV || border_strategy == BORDER_CDJV_COND) {

//		printf("Allocating wsvector...\n");fflush(stdout);

		/* Prepare the working vector here. It shall hold the preconditioned signal
		 * We allocate this vector only once per dimension and reuse the memory for all following computations
		 */
		float* wsvector = (float*) malloc((signallen) * sizeof(float));
		assert(wsvector!=NULL);

		switch (desc->ndims) {

		case 1:
			dwt_real_dec_1d_cdjv(data, signallen, gap, filters, cdjv, filterlen, wsvector);
			break;

		case 2:
			for (i = 0; i < curr_signal_lens[1 - dim]; i++)
				dwt_real_dec_1d_cdjv(data +
					i * desc->gaps[1 - dim],
					signallen, gap, filters, cdjv, filterlen, wsvector);
			break;

		case 3:
			for (i = 0; i < curr_signal_lens[(1 + dim)%3]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%3]; j++)
					dwt_real_dec_1d_cdjv(data +
						i * desc->gaps[(1 + dim)%3] +
						j *desc->gaps[(2 + dim)%3],
						signallen, gap, filters, cdjv, filterlen, wsvector);

			break;

		case 4:
			for	(i = 0; i < curr_signal_lens[(1 + dim)%4]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%4]; j++)
					for (k = 0; k < curr_signal_lens[(3 + dim)%4]; k++)
						dwt_real_dec_1d_cdjv(data +
							i * desc->gaps[(1 + dim)%4] +
							j * desc->gaps[(2 + dim)%4] +
							k * desc->gaps[(3 + dim)%4],
							signallen, gap, filters, cdjv, filterlen, wsvector);

			break;
		}

		free(wsvector);

	} else {

//		printf("Allocating wsvector...\n");fflush(stdout);

		/* Prepare the working vector here. It shall hold the signal preceded by filterlength-1 elements
		 * added for border distortions enhancements, according to the specified border strategy
		 * We allocate this vector only once per dimension and reuse the memory for all following computations
		 */
		float* wsvector = (float*) malloc((signallen + 2 * filterlen) * sizeof(float));
		assert(wsvector!=NULL);

		switch (desc->ndims) {

		case 1:
			dwt_real_dec_1d(data, signallen, gap, filters, nbands, filterlen, border_strategy, wsvector);
			break;

		case 2:
			for (i = 0; i < curr_signal_lens[1 - dim]; i++)
				dwt_real_dec_1d(data +
						i * desc->gaps[1 - dim],
						signallen, gap, filters, nbands, filterlen, border_strategy, wsvector);
			break;

		case 3:
			for (i = 0; i < curr_signal_lens[(1 + dim)%3]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%3]; j++)
					dwt_real_dec_1d(data +
							i * desc->gaps[(1 + dim)%3] +
							j * desc->gaps[(2 + dim)%3],
							signallen, gap, filters, nbands, filterlen, border_strategy, wsvector);
			break;

		case 4:
			for	(i = 0; i < curr_signal_lens[(1 + dim)%4]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%4]; j++)
					for (k = 0; k < curr_signal_lens[(3 + dim)%4]; k++)
						dwt_real_dec_1d(data +
								i * desc->gaps[(1 + dim)%4] +
								j * desc->gaps[(2 + dim)%4] +
								k * desc->gaps[(3 + dim)%4],
								signallen, gap, filters, nbands, filterlen, border_strategy, wsvector);
			break;
		}

		free(wsvector);
	}

}

inline void dwt_real_dec_1d(float* data, int signallen, int gap, float** filters, int nbands, int filterlen, dwt_border_t border_strategy, float* wsvector) {

	int i;

	int padlen;


//	printf("Padding...\n");fflush(stdout);

	// Now we do the padding, according to the border strategy
	switch (border_strategy) {

	case BORDER_SYM_HALF:

		padlen = filterlen-1;

		// Pad left
		for (i = 0; i < padlen; i++)
			wsvector[i] = data[(padlen -  i - 1)*gap];

		// Signal
		for (i = 0; i < signallen; i++)
			wsvector[padlen + i] = data[i * gap];

		// Pad right
		for (i = 0; i < padlen; i++)
			wsvector[padlen + signallen + i] = data[(signallen - i - 1)*gap];

		break;

	case BORDER_SYM_WHOLE:

		padlen = filterlen-1;

		// Pad left
		for (i = 0; i < padlen; i++)
			wsvector[i] = data[(padlen - i)*gap];

		// Signal
		for (i = 0; i < signallen; i++)
			wsvector[padlen + i] = data[i * gap];

		// Pad right
		for (i = 0; i < padlen; i++)
			wsvector[padlen + signallen + i] = data[(signallen - i - 2)*gap];

		break;

	case BORDER_PERIOD:

		padlen = filterlen/2;

		// Pad left
		for (i = 0; i < padlen; i++)
			wsvector[i] = data[(signallen - padlen + i)*gap];

		// Signal
		for (i = 0; i < signallen; i++)
			wsvector[padlen + i] = data[i * gap];

		// Pad right
		for (i = 0; i < padlen; i++)
			wsvector[padlen + signallen + i] = data[i*gap];

		break;

	default:
		/**
		 * BORDER_PAD_ZERO or anything unknown
		 */

		padlen = filterlen - 1;

		// Pad left
		for (i = 0; i < padlen; i++)
			wsvector[i] = 0;

		// Signal
		for (i = 0; i < signallen; i++)
			wsvector[padlen + i] = data[i * gap];

		// Pad right
		for (i = 0; i < padlen; i++)
			wsvector[padlen + signallen + i] = 0;

	}

//	int counter;
//	for(counter=0;counter<signallen + 2*padlen;counter++)
//		printf("%.2f  ", wsvector[counter]);
//	printf("\n");


	/**
	 * Compute number of samples that shall correspond to each band in the result vector
	 */
	int bandlen = signallen / nbands;

//	printf("Convolving...\n");fflush(stdout);

	/**
	 * Now we are ready to convolve and downsample the signal. These operations will be done in the same step.
	 * This way we avoid computing coefficents that would later be discarded anyway. The basic idea is that the first
	 * signallen/nbands coefficients are obtained by convolving with first band's filter and so on. We can find out for
	 * each location in the signal vector (which will be replaced by a DWT coefficient), which filter we need to convolve
	 * and at which position.
	 */
	for (i = 0; i < signallen; i++)
	/**
	 * i/filter->nbands gives the current band
	 * (i%bandlen)*filter->nbands gives the element of the signal that generates element number i of the result
	 */
	{
		// TODO: here I do not agree with the matlab implementation, since it actually boils down to decimate by
		// keeping the even numbered elements - for this implementation. This is an open issue. In this  form
		// the output of this implementation coincides with matlab, otherwise remove +1 in the last argument to dotprod_real
		data[i * gap] = dotprod_real(filterlen, filters[i / bandlen], wsvector, (i % bandlen) * nbands + 1);
//		printf("i*gap = %d; filterlen=%d, i/nbands=%d; i%%bandlen*nbands+1=%d\n", i * gap, filterlen,i/bandlen, (i%bandlen)*nbands+1);
//		fflush(stdout);
	}

}


inline void dwt_real_dec_1d_cdjv(float* data, int signallen, int gap, float** filters, dwt_cdjv_t* cdjv, int filterlen, float* wsvector) {

	int i, j;
	int offset = signallen - cdjv->degree;

//	printf("cdjv degree: %d\n", cdjv->degree);

//	printf("Filling wsvector ...\n");fflush(stdout);

	for (i = 0; i < signallen; i++)
		wsvector[i] = data[i * gap];

//	int counter;
//	printf("wsvector:\n");
//	for(counter=0;counter<signallen;counter++)
//		printf("%.2f ", wsvector[counter]);
//	printf("\n"); fflush(stdout);

	int bandlen = signallen/2;
	offset = bandlen - cdjv->degree;

	float* approx = data;
	float* detail = data + bandlen * gap;

//	printf("Convolving left edge...\n");fflush(stdout);

	for (i = 0; i < cdjv->degree; i++) {
		approx[i * gap] = detail[i * gap] = 0;
		for (j = 0; j < 3 * cdjv ->degree - 1; j++) {
			approx[i * gap] += cdjv->leftlopass[i][j] * wsvector[j];
			detail[i * gap] += cdjv->lefthipass[i][j] * wsvector[j];
		}
	}

//	printf("Convolving middle...\n");fflush(stdout);

	for (i = cdjv->degree; i < offset; i++) {
		approx[i * gap] = dotprod_real(filterlen, filters[0], wsvector - cdjv->degree, 2*i + 1);
		detail[i * gap] = dotprod_real(filterlen, filters[1], wsvector - cdjv->degree, 2*i + 1);
	}

//	printf("Convolving right edge...\n");fflush(stdout);

	int bigoffset = signallen - 3*cdjv->degree + 1;

	for (i = 0; i < cdjv->degree; i++) {
		approx[(offset + i) * gap] = detail[(offset + i) * gap] = 0;
		for (j = 0; j < 3* cdjv ->degree - 1; j++) {
			approx[(offset + i) * gap] += cdjv->rightlopass[i][j] * wsvector[bigoffset + j];
			detail[(offset + i) * gap] += cdjv->righthipass[i][j] * wsvector[bigoffset + j];
		}
	}
}


/* * RECONSTRUCTION * */

void dwt_real_reconstruction(float* data, mri_matrix_descriptor_t* desc, dwt_params_t* params) {

	// Loop counters
	int rl, dim;

	/* Checking assumptions validity */

	// The number of dimensions (DWT and input data) must agree
	assert(desc->ndims == params->ndims);

	// Checks per dimension
	for (dim = 0; dim < desc->ndims; dim++) {

		// The data length must be a multiple of nbands^resolution
		assert(desc->dims[dim] % (int) pow(params->filters[dim]->nbands, params->res_levels[dim]) == 0);

		// At the highest resolution level we need at least filterlen elements left
		assert(desc->dims[dim] / (int) pow(params->filters[dim]->nbands, params->res_levels[dim]) >= params->filters[dim]->length);

		// CDJV check - only 2band transform
		if(params->border_strategies[dim] == BORDER_CDJV || params->border_strategies[dim] == BORDER_CDJV_COND) {
			assert(params->filters[dim]->nbands == 2);
			assert(params->filters[dim]->cdjv != NULL);
		}
	}

//	printf("IDWT: Initializing resolution level vector...\n"); fflush(stdout);

	// This vector holds the current level of resolution for each dimension
	int* curr_res_levels = (int*) malloc(params->ndims * sizeof(int));
	assert(curr_res_levels != NULL);

	// This vector holds the current signal length for each dimension
	// (shall increase as resolution level decreases)
	int* curr_signal_lens = (int*) malloc(params->ndims * sizeof(int));
	assert(curr_signal_lens != NULL);

	// This is the needed number of resolution iterations - corresponding to
	// the dimension with the maximum number of res_levels
	int max_res_level = 0;

	// The max resolution level at the current reconstruction step
	int curr_max_level = 0;

	// Initialize resolution levels and compute max.
	for (dim = 0; dim < desc->ndims; dim++) {

		curr_res_levels[dim] = params->res_levels[dim];

		if (params->res_levels[dim] > max_res_level)
			max_res_level = params->res_levels[dim];

	}

//	printf("max_res_level=%d\n", max_res_level);fflush(stdout);

	// Iterate through resolution levels
	for (rl = 0; rl < max_res_level; rl++) {

//		printf("rl=%d: ", rl);

		// Compute current max. level and signal lengths; on this rl iteration we should
		// perform reconstruction only on the dimensions that are at the
		// current maximum decomposition level
		curr_max_level = 0;
		for (dim = 0; dim < desc->ndims; dim++)
			if (curr_max_level < curr_res_levels[dim])
				curr_max_level = curr_res_levels[dim];

//		printf("curr_max_level=%d\n", curr_max_level);

		for (dim = 0; dim < desc->ndims; dim++) {
			if (curr_res_levels[dim] == curr_max_level) {

				int dim1;
				for (dim1 = 0; dim1 < desc->ndims; dim1++)
					if (curr_res_levels[dim1] == curr_res_levels[dim])
						curr_signal_lens[dim1] = desc->dims[dim1] / pow(params->filters[dim1]->nbands, curr_res_levels[dim1]-1);
					else
						curr_signal_lens[dim1] = desc->dims[dim1] / pow(params->filters[dim1]->nbands, curr_res_levels[dim1]);

				// On this dimension we need to apply the transform
				dwt_real_rec_along_dim(data, desc, dim, params->filters[dim], params->border_strategies[dim], curr_signal_lens);
				curr_res_levels[dim]--;
			}
		}
	}

	free(curr_res_levels);
	free(curr_signal_lens);


	// In case there is any CDJV wavelet for the intervals
	// do the postconditioning (if the flag is set)
//	printf("Postconditioning...\n");fflush(stdout);
	for(dim=0;dim<desc->ndims;dim++)
		if(params->border_strategies[dim] == BORDER_CDJV_COND)
			dwt_real_cond_cdjv_along_dim(data, dim, desc,
					params->filters[dim]->cdjv->degree,
					params->filters[dim]->cdjv->leftpostcond,
					params->filters[dim]->cdjv->rightpostcond);
}

/**
 * Performs IDWT along dimension dim of the multidimensional matrix data
 */
void dwt_real_rec_along_dim(float* data, mri_matrix_descriptor_t* desc, int dim, dwt_filter_t* filter, dwt_border_t border_strategy, int* curr_signal_lens) {

//	printf("Applying IDWT along dimension #%d of the input data using filter %s"
//		" and border strategy %s\n", dim + 1, filter->name, DWT_BORDER_TO_STR(border_strategy));
//	printf("Current signal lengths on each dimension:\n");
//	int cntr;
//	for (cntr = 0; cntr < desc->ndims; cntr++)
//		printf(" %d ", curr_signal_lens[cntr]);
//	printf("\nPrecalculating variables...\n");
//	fflush(stdout);

	/**
	 * Some useful stuff precalculated in local variables, for speed and simplicity
	 */
	int nbands = filter->nbands;
	int signallen = curr_signal_lens[dim];
	int gap = desc->gaps[dim];
	float** filters = filter->r_filters;
	dwt_cdjv_t* cdjv = filter->cdjv;
	int filterlen = filter->length;

//	printf("=> signallen=%d, gap=%d\n", signallen, gap);

	if (border_strategy == BORDER_CDJV || border_strategy == BORDER_CDJV_COND) {

		int padlen = filterlen - 1;

//		printf("Allocating wsvectors...\n");fflush(stdout);

		float** wsvectors = (float**) malloc(2 * sizeof(float*));
		assert(wsvectors!=NULL);
		wsvectors[0] = (float*) malloc((2*padlen + signallen - 4*cdjv->degree) * sizeof(float));
		assert(wsvectors[0]!=NULL);
		wsvectors[1] = (float*) malloc((2*padlen + signallen - 4*cdjv->degree) * sizeof(float));
		assert(wsvectors[1]!=NULL);

		float** edges = (float**) malloc(4 * sizeof(float*));
		assert(edges != NULL);
		edges[0] = (float*) malloc(cdjv->degree * sizeof(float));
		assert(edges[0]);
		edges[1] = (float*) malloc(cdjv->degree * sizeof(float));
		assert(edges[1]);
		edges[2] = (float*) malloc(cdjv->degree * sizeof(float));
		assert(edges[2]);
		edges[3] = (float*) malloc(cdjv->degree * sizeof(float));
		assert(edges[3]);

		int i, j, k;

		switch (desc->ndims) {

		case 1:
			dwt_real_rec_1d_cdjv(data, signallen, gap, filters, cdjv, filterlen, padlen, wsvectors, edges);
			break;

		case 2:
			for (i = 0; i < curr_signal_lens[1 - dim]; i++)
				dwt_real_rec_1d_cdjv(data +
						i * desc->gaps[1 - dim],
						signallen, gap, filters, cdjv, filterlen, padlen, wsvectors, edges);
			break;

		case 3:
			for (i = 0; i < curr_signal_lens[(1 + dim)%3]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%3]; j++)
					dwt_real_rec_1d_cdjv(data +
							i * desc->gaps[(1 + dim)%3] +
							j * desc->gaps[(2 + dim)%3],
							signallen, gap, filters, cdjv, filterlen, padlen, wsvectors, edges);
			break;

		case 4:
			for	(i = 0; i < curr_signal_lens[(1 + dim)%4]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%4]; j++)
					for (k = 0; k < curr_signal_lens[(3 + dim)%4]; k++)
						dwt_real_rec_1d_cdjv(data +
								i * desc->gaps[(1 + dim)%4] +
								j * desc->gaps[(2 + dim)%4] +
								k * desc->gaps[(3 + dim)%4],
								signallen, gap, filters, cdjv, filterlen, padlen, wsvectors, edges);
			break;
		}

		free(wsvectors[0]);
		free(wsvectors[1]);
		free(wsvectors);

		free(edges[0]);
		free(edges[1]);
		free(edges[2]);
		free(edges[3]);
		free(edges);

	} else {


	//	printf("Allocating wsvectors...\n");
	//	fflush(stdout);

		/* Prepare the working vectors here. They shall hold the signal preceded by filterlength-1 elements
		 * added for border distortions enhancements, according to the specified border strategy
		 * We allocate these vectors only once per dimension and reuse the memory for all following computations
		 */
		int band;
		float** wsvectors = (float**) malloc(nbands * sizeof(float*));
		assert(wsvectors != NULL);
		for (band = 0; band < nbands; band++) {
			wsvectors[band] = (float*) malloc((signallen + 2*filter->length) * sizeof(float));
			assert(wsvectors[band] != NULL);
		}

		/**
		 * We now need to apply the DWT on each 1D vector of this dimension, which
		 * implies iterating through the space of all the other dimensions. This
		 * could be implemented for the n-dimensional case, but would waste a lot
		 * of time with iterating an n-1 dimensional space. Therefore, each case
		 * is implemented separately
		 */

		//	printf("Performing IDWT...\n");fflush(stdout);

		int i, j, k;

		switch (desc->ndims) {

		case 1:
			dwt_real_rec_1d(data, signallen, gap, filters, nbands, filterlen, border_strategy, wsvectors);
			break;

		case 2:
			for (i = 0; i < curr_signal_lens[1 - dim]; i++)
				dwt_real_rec_1d(data +
						i * desc->gaps[1 - dim],
						signallen, gap, filters, nbands, filterlen, border_strategy, wsvectors);
			break;

		case 3:
			for (i = 0; i < curr_signal_lens[(1 + dim)%3]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%3]; j++)
					dwt_real_rec_1d(data +
							i * desc->gaps[(1 + dim)%3] +
							j * desc->gaps[(2 + dim)%3],
							signallen, gap, filters, nbands, filterlen, border_strategy, wsvectors);
			break;

		case 4:
			for	(i = 0; i < curr_signal_lens[(1 + dim)%4]; i++)
				for (j = 0; j < curr_signal_lens[(2 + dim)%4]; j++)
					for (k = 0; k < curr_signal_lens[(3 + dim)%4]; k++)
						dwt_real_rec_1d(data +
								i * desc->gaps[(1 + dim)%4] +
								j * desc->gaps[(2 + dim)%4] +
								k * desc->gaps[(3 + dim)%4],
								signallen, gap, filters, nbands, filterlen, border_strategy, wsvectors);
			break;
		}

		for (i = 0; i < nbands; i++)
			free(wsvectors[i]);
		free(wsvectors);

	}
}

inline void dwt_real_rec_1d(float* data, int signallen, int gap, float** filters, int nbands, int filterlen, dwt_border_t border_strategy, float** wsvectors) {

	int i, band, padlen;

//	printf("signallen=%d, nbands=%d\n", signallen, nbands);fflush(stdout);
//	printf("\nFilling wsvector...\n");fflush(stdout);

	/**
	 * Compute number of samples that shall correspond to each band
	 */
	int bandlen = signallen / nbands;

	// Clear wsvector to 0
	for (band = 0; band < nbands; band++)
		memset(wsvectors[band], 0, (signallen + 2*filterlen) * sizeof(float));

//	printf("Padding...\n");fflush(stdout);

	// Now we do the padding, according to the border strategy, for each wsvector
	for (band = 0; band < nbands; band++) {

		switch (border_strategy) {

		case BORDER_SYM_HALF:

			padlen = filterlen-1;

			// Signal
			for (i = 0; i < bandlen; i++)
				wsvectors[band][padlen + nbands*i] = data[(band * bandlen + i) * gap];

			// Pad left
			for (i = 0; i < padlen; i++)
				wsvectors[band][i] = wsvectors[band][2*padlen -  i - 1];

			// Pad right
			for (i = 0; i < padlen; i++)
				wsvectors[band][padlen + signallen + i] = wsvectors[band][padlen + signallen - i - 1];

			break;

		case BORDER_SYM_WHOLE:

			padlen = filterlen-1;

			// Signal
			for (i = 0; i < bandlen; i++)
				wsvectors[band][padlen + nbands*i] = data[(band * bandlen + i) * gap];

			// Pad left
			for (i = 0; i < padlen; i++)
				wsvectors[band][i] = wsvectors[band][2*padlen -  i];

			// Pad right
			for (i = 0; i < padlen; i++)
				wsvectors[band][padlen + signallen + i] = wsvectors[band][padlen + signallen - i - 2];

			break;

		case BORDER_PERIOD:

			padlen = filterlen/2;

			// Signal
			for (i = 0; i < bandlen; i++)
				wsvectors[band][padlen + nbands*i] = data[(band * bandlen + i) * gap];

			// Pad left
			for (i = 0; i < padlen; i++)
				wsvectors[band][i] = wsvectors[band][signallen + i];

			// Pad right
			for (i = 0; i < padlen; i++)
				wsvectors[band][padlen + signallen + i] = wsvectors[band][padlen + i];

			break;

		default:
			/**
			 * BORDER_PAD_ZERO or anything unknown
			 */

			padlen = filterlen - 1;

			// Signal
			for (i = 0; i < bandlen; i++)
				wsvectors[band][padlen + nbands*i] = data[(band * bandlen + i) * gap];

			// Pad left
			for (i = 0; i < padlen; i++)
				wsvectors[band][i] = 0;

			// Pad right
			for (i = 0; i < padlen; i++)
				wsvectors[band][padlen + signallen + i] = 0;

		}

	}

//	printf("\n");
//	for (band = 0; band < nbands; band++) {
//		printf("wsvectors[%d]=", band);
//		for (i = 0; i < signallen + 2*padlen; i++) {
//			printf(" %.2f  ", wsvectors[band][i]);
//		}
//		printf("\n");
//	}
//	printf("\n");
//	printf("Convolving...\n");
//	fflush(stdout);

	/**
	 * Now we are ready to perform the convolution and compute the reconstructed result.
	 */
	for (i = 0; i < signallen; i++) {
		data[i * gap] = 0;
		for (band = 0; band < nbands; band++) {
			data[i * gap] += dotprod_real(filterlen, filters[band], wsvectors[band], i);
//			printf("band=%d, i=%d, dotprod_real=%f \n", band, i, data[i * gap]);fflush(stdout);
		}
	}

}


inline void dwt_real_rec_1d_cdjv(float* data, int signallen, int gap, float** filters, dwt_cdjv_t* cdjv, int filterlen, int padlen, float** wsvectors, float** edges) {

	int i, j;
	int bandlen = signallen/2;
	int offset;

//	printf("cdjv degree=%d, bandlen=%d\n", cdjv->degree, bandlen);


//	printf("Filling edges ...\n");fflush(stdout);
	for(i=0;i<cdjv->degree;i++) {
		// Left edges
		edges[0][i] = data[i * gap];
		edges[1][i] = data[(bandlen+i) * gap];
		// Right edges
		edges[2][i] = data[(bandlen-cdjv->degree+i) * gap];
		edges[3][i] = data[(signallen-cdjv->degree+i) * gap];
	}

	// Clear wsvectors to 0
	memset(wsvectors[0], 0, (2*padlen + signallen - 4*cdjv->degree) * sizeof(float));
	memset(wsvectors[1], 0, (2*padlen + signallen - 4*cdjv->degree) * sizeof(float));

//	printf("Filling wsvectors ...\n");fflush(stdout);
	for (i = 0; i < bandlen - 2*cdjv->degree; i++) {
		wsvectors[0][padlen + 2*i] = data[(cdjv->degree + i) * gap];
		wsvectors[1][padlen + 2*i] = data[(bandlen + cdjv->degree + i) * gap];
	}

	// Clear data to 0
	for(i=0;i<signallen;i++)
		data[i*gap] = 0;

//	printf("Convolving left edge...\n");fflush(stdout);

	for (i = 0; i < 3*cdjv->degree-1; i++) {
		for (j = 0; j < cdjv->degree; j++)
			data[i * gap] +=
				cdjv->leftlopass[j][i] * edges[0][j] +
				cdjv->lefthipass[j][i] * edges[1][j];
	}

//	printf("Convolving right edge...\n");fflush(stdout);

	offset = signallen - 3*cdjv->degree + 1;

	for (i = 0; i < 3*cdjv->degree-1; i++) {
		for (j = 0; j < cdjv->degree; j++)
			data[(offset + i) * gap] +=
				cdjv->rightlopass[j][i] * edges[2][j] +
				cdjv->righthipass[j][i] * edges[3][j];
	}

//	printf("Convolving middle...\n");fflush(stdout);

	for (i = 0; i < padlen + signallen - 4*cdjv->degree; i++)
		data[(i + cdjv->degree + 1)*gap] +=
			dotprod_real(filterlen, filters[0], wsvectors[0], i) +
			dotprod_real(filterlen, filters[1], wsvectors[1], i);

//	int counter;
//	printf("wsvectors:\n");
//	for(counter=0;counter<signallen - 4*cdjv->degree + 2*padlen;counter++)
//		printf("%.2f ", wsvectors[0][counter]);
//	printf("\n"); fflush(stdout);
//	for(counter=0;counter<signallen - 4*cdjv->degree + 2*padlen;counter++)
//		printf("%.2f ", wsvectors[1][counter]);
//	printf("\n"); fflush(stdout);

}




// CDJV data pre/post conditioning along the specifid dimension
void dwt_real_cond_cdjv_along_dim(float* data, int dim, mri_matrix_descriptor_t* desc, int degree, float** leftmat, float** rightmat) {

	int i, j, k;

	float* wsvector = (float*)malloc(degree * sizeof(float));

	switch(desc->ndims) {
	case 1:
		dwt_real_cond_cdjv_1d(data, 1, desc->dims[0], degree, leftmat, rightmat, wsvector);
		break;

	case 2:
		for(i=0;i<desc->dims[1-dim];i++)
			dwt_real_cond_cdjv_1d(data +
					i * desc->gaps[1-dim],
					desc->gaps[dim], desc->dims[dim], degree, leftmat, rightmat, wsvector);
		break;

	case 3:
		for(i=0;i<desc->dims[(1 + dim)%3];i++)
			for(j=0;j<desc->dims[(2 + dim)%3];j++)
				dwt_real_cond_cdjv_1d(data +
						i * desc->gaps[(1 + dim)%3] +
						j * desc->gaps[(2 + dim)%3],
						desc->gaps[dim], desc->dims[dim], degree, leftmat, rightmat, wsvector);
		break;

	case 4:
		for	(i = 0; i < desc->dims[(1 + dim)%4]; i++)
			for (j = 0; j < desc->dims[(2 + dim)%4]; j++)
				for (k = 0; k < desc->dims[(3 + dim)%4]; k++)
					dwt_real_cond_cdjv_1d(data +
							i * desc->gaps[(1 + dim)%4] +
							j * desc->gaps[(2 + dim)%4] +
							k * desc->gaps[(3 + dim)%4],
							desc->gaps[dim], desc->dims[dim], degree, leftmat, rightmat, wsvector);
		break;

	default:
		assert("Unsupported dimension"==NULL);
		break;
	}

	free(wsvector);

}

// CDJV data pre/post conditioning for 1d signal
void dwt_real_cond_cdjv_1d(float* data, int gap, int len, int degree, float** leftmat, float** rightmat, float* wsvector) {

	int i, j;

//	printf("Filling condition wsvector...\n");
	for(i = 0; i < degree; i++)
		wsvector[i] = data[i*gap];

//	printf("Conditioning left...\n");fflush(stdout);
	for(i = 0; i < degree; i++) {
		data[i*gap] = 0;
		for(j = 0; j < degree; j++)
			data[i*gap] += leftmat[i][j] * wsvector[j];
	}

//	printf("Filling condition wsvector...\n");
	for(i = 0; i < degree; i++)
		wsvector[i] = data[(len - degree + i)*gap];

//	printf("Conditioning right...\n");fflush(stdout);
	for(i = 0; i < degree; i++) {
		data[(len - degree + i)*gap] = 0;
		for(j = 0; j < degree; j++)
			data[(len - degree + i)*gap] += rightmat[i][j] * wsvector[j];
	}

}



inline float dotprod_real(int len, float* a, float* b, int n) {

	int i;

	float result = 0;

	// Shifting a means increasing start point in b
	b += n;

	for (i = 0; i < len; i++)
		result += a[i] * b[i];

	return result;
}
